#include <stdlib.h>

//#include "tarm_timer.h"
#include "TARM_Middleware/tarm_math.h"

#include "TARM_Hardware/tarm_io.h"
#include "TARM_Hardware/tarm_limit.h"
#include "TARM_Hardware/tarm_pwm.h"
#include "TARM_Hardware/tarm_baseTimer.h"

//#include "TARM_Application/tarm_init.h"
#include "TARM_Application/tarm_run.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

//#include "hardware/closeloop/CloseLoop.h"


/************ function **************/
//******************************
//@brief 		闭环反馈
//@param[in]
//@return
//******************************
void CloseLoopUpdate()
{
//	if(CloseLoop_Gather() != 1){
//		printf("无闭环：error");
//		return;
//	}

////	printf("闭环结果：\tprime:%f;\r\n\t\tvice:%f",CloseLoopSingle.prime, CloseLoopSingle.vice);

//	g_TARM_RunRecord.pv_current.p = CloseLoopSingle.prime;
//	g_TARM_RunRecord.pv_current.v = CloseLoopSingle.vice;
//	angle_cur_to_xy();

//	printf("\nClose Loop Update\n");

//	ReportStruct.Uart1ReportFlag.CloseLoopReport = 1;
//	ReportStruct.Uart2ReportFlag.CloseLoopReport = 1;
}

/************************************************************
*	复位函数，初始化、碰到限位时使用
*	将目标、现时的xy、angle值全部复位
	除能TIM2、3中断
************************************************************/
//void reset_point(void)
//{
//	g_TARM_RunRecord.pv_distance.p = g_TARM_Factor.limit_p;
//	g_TARM_RunRecord.pv_current.p = g_TARM_Factor.limit_p;
//	g_TARM_RunRecord.pv_distance.v = g_TARM_Factor.limit_v;
//	g_TARM_RunRecord.pv_current.v = g_TARM_Factor.limit_v;
//
//	angle_des_to_xy();
//	g_TARM_RunRecord.pv_current.x = g_TARM_RunRecord.pv_distance.x;
//	g_TARM_RunRecord.pv_current.y = g_TARM_RunRecord.pv_distance.y;
//
//	//TIM_Cmd(TIM2, DISABLE);
//	//TIM_Cmd(TIM3, DISABLE);
//}

void reset_point_prime(void)
{
//	TimeBase_Disable();
//	TimePV_Disable();
	g_TARM_RunRecord.RunFlag_p = 0;

	g_TARM_RunRecord.pv_distance.p = g_TARM_Factor.limit_p;
	g_TARM_RunRecord.pv_current.p = g_TARM_Factor.limit_p;
	angle_des_to_xy();
	g_TARM_RunRecord.pv_current.x = g_TARM_RunRecord.pv_distance.x;
	g_TARM_RunRecord.pv_current.y = g_TARM_RunRecord.pv_distance.y;

	printf("\r\n\tlimit_prime!!!!!!!!!!\r\n");

//	Limit_Disable();
	goto_null_point();
//	Limit_Enable();
}

void reset_point_vice(void)
{
//	TimeBase_Disable();
//	TimePV_Disable();
	g_TARM_RunRecord.RunFlag_v = 0;

	g_TARM_RunRecord.pv_distance.v = g_TARM_Factor.limit_v;
	g_TARM_RunRecord.pv_current.v = g_TARM_Factor.limit_v;
	angle_des_to_xy();
	g_TARM_RunRecord.pv_current.x = g_TARM_RunRecord.pv_distance.x;
	g_TARM_RunRecord.pv_current.y = g_TARM_RunRecord.pv_distance.y;

	printf("\r\n\tlimit_vice!!!!!!!!!!!!\r\n");

//	Limit_Disable();
	goto_null_point();
//	Limit_Enable();
}

/************************************************************
*	回到零点
*	（null_point）：g_TARM_Factor.null_p=60,g_TARM_Factor.null_v=40;
*	参考点是g_TARM_Factor.limit_p=57,g_TARM_Factor.limit_v=32;
************************************************************/
void goto_null_point(void)
{
//	point_to_point_angle(g_TARM_Factor.null_p,g_TARM_Factor.null_v,200);
	handle_pvMainLoop(17, 12, 150);
}

/************************************************************
*	夹持
************************************************************/
//void click_pwm(void)
//{
//	double duty_cycle = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0;
//	double duty_time = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0*20;

//	PwmClick_ChangeValue(g_TARM_RunRecord.ClickNum);

//	printf("\n pwm change,占空比：%f;占空时间：%f \r\n", duty_cycle, duty_time);
//}

//void turn_pwm(void)
//{
//	double duty_cycle = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0;
//	double duty_time = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0*20;

//	PwmTurn_ChangeValue(g_TARM_RunRecord.TurnNum);

//	printf("\n pwm change,占空比：%f;占空时间：%f \r\n", duty_cycle, duty_time);
//}
